/*LICENSE_BEGIN
 * ******************************************************************
 * Copyright 2006 by the Dainamite Robocup-Team at the
 * DAI-Labor, TU-Berlin (www.dai-labor.de)
 * 
 * Members of Dainamite are:
 * Holger Endert
 * Robert Wetzker
 * Thomas Karbe
 * Axel Heßler
 * Philippe Bütter
 * Felix Brossmann
 * 
 * Licensed under the GNU GPL (General Public License) Free Software 
 * License, Version 2. You may use this file only in compliance with 
 * the License. You may obtain a copy of the License at:
 * 
 * http://www.fsf.org/licensing/licenses/gpl.txt
 * ******************************************************************
LICENSE_END*/
package robocup.component.situations;

import static java.lang.Math.abs;

import java.util.LinkedList;

import robocup.component.Prophet;
import robocup.component.SConf;
import robocup.component.WorldModel;
import robocup.component.ControllerNumbers.PLAY_MODE;
import robocup.component.ControllerNumbers.STAMINA_LEVEL;
import robocup.component.actionfactories.BasicActionFactory;
import robocup.component.actions.Action;
import robocup.component.actions.CatchAction;
import robocup.component.actions.DashAction;
import robocup.component.actions.KickAction;
import robocup.component.actions.TurnAction;
import robocup.component.geometry.Vektor;
import robocup.component.worldobjects.Ball;
import robocup.component.worldobjects.Line;
import robocup.component.worldobjects.Player;
import robocup.component.worldobjects.ReachableArea;
import robocup.component.worldobjects.Player.RELATIVE;

/**
 * The best action of the goalie is calculated here.
 * 
 * @author Thomas Karbe
 * 
 */
public class GoalkeeperSituation extends Situation {

	private static String DEFENSIVE = "defensive";

	private static String INTERCEPTION = "interception";

	private static String CATCH = "catch";

	private static String KICK = "kick";

	private static String INTERCEPT_GOAL_BALL = "intercept goal ball";

	private static String STANDARD = "standard";

	private static final double CATCH_SECURITY_DIST = 0.12;

	private static final double KICK_SECURITY_DIST = 0.05;

	private static final int INTERCEPTION_SECURITY = 2;

	private static final double PASS_BENEFIT = 0.2;

	private static final double DANGEROUS_DIST = 28.0;

	private static final double IS_DEFENDED_DIST = 4.0;

	private static final double GOOD_POS_BUFFER = 1.0;

	private static final Vektor ANCHOR = new Vektor(Vektor.XY, 0, -57);

	private boolean debug = false;

	private boolean debugDef = false;

	Prophet prophet;

	SConf sConf = null;

	PLAY_MODE playmode = null;

	Player keeperRef = null;

	Ball ballRef = null;

	Player fastOpp = null;

	Vektor enemyPos = null;

	Vektor bestDefPos = Vektor.getDummyInstance();

	int fastOppIT = 0;

	Line dummyLine1 = new Line(Vektor.getDummyInstance(), Vektor
			.getDummyInstance());

	Line dummyLine2 = new Line(Vektor.getDummyInstance(), Vektor
			.getDummyInstance());

	Line shortLine = new Line(Vektor.getDummyInstance(), Vektor
			.getDummyInstance());

	Line longLine = new Line(Vektor.getDummyInstance(), Vektor
			.getDummyInstance());

	Line middleLine = new Line(Vektor.getDummyInstance(), Vektor
			.getDummyInstance());

	Line defenseLine = new Line(Vektor.getDummyInstance(), Vektor
			.getDummyInstance());

	Vektor dummyVektor = Vektor.getDummyInstance();

	Vektor dummyVektor1 = Vektor.getDummyInstance();

	Vektor dummyVektor2 = Vektor.getDummyInstance();

	Vektor dummyVektor3 = Vektor.getDummyInstance();

	Vektor dummyVektor4 = Vektor.getDummyInstance();

	Ball dummyBall1 = Ball.getDummyInstance();

	Ball dummyBall2 = Ball.getDummyInstance();

	Player dummyPlayer1 = Player.getDummyInstance();

	ReachableArea ra = new ReachableArea();

	Vektor leftPostRef = Vektor.getDummyInstance();

	Vektor rightPostRef = Vektor.getDummyInstance();

	Vektor shortPost = Vektor.getDummyInstance();

	Vektor longPost = Vektor.getDummyInstance();

	DashAction dashAction = new DashAction(0);

	TurnAction turnAction = new TurnAction(0);

	boolean ballGoesOut = false;

	/**
	 * Creates a new GoalkeeperSituation - a situation which calculates how to
	 * behave as goalie in different situations
	 * 
	 * @param world -
	 *            the current world-model
	 * @param movements -
	 *            a generic movement model
	 * @param _prophet -
	 *            the parent object of this
	 */
	public GoalkeeperSituation(WorldModel world, Movements movements,
			Prophet _prophet) {

		super(world, movements);
		prophet = _prophet;
		update();
	}

	@Override
	protected void update() {

		this.sConf = SConf.getInstance();
		playmode = getWorld().getPlayMode();
		keeperRef = getWorld().getSelfRef();
		if (!keeperRef.isGoalie()) {
			System.out.println("GoalkeeperSituation is used by invalid player");
		}
		ballRef = getWorld().getBallRef();
		fastOpp = prophet.getFastestEnemyToBall();
		fastOppIT = fastOpp == null ? -1 : prophet.getInterceptionTime(fastOpp
				.getNumber(), false);
		leftPostRef = sConf.GOAL_POST_LEFT_OWN;
		rightPostRef = sConf.GOAL_POST_RIGHT_OWN;
		ballGoesOut = prophet.ballOutBeforeIntercept();

		calculateBestAction();
		super.update();
	}

	private void calculateBestAction() {

		long time = System.currentTimeMillis();
		benefit = 1.;
		if (debug)
			System.out.println("\n" + getWorld().getCycleAndPlayerString());
		if (playmode == PLAY_MODE.PLAY_ON) {
			if (canCatch()) {
				bestAction = catchBallAction();
				subStateString = CATCH;
				if (debug)
					System.out.println("catch ball " + bestAction);
			}// end can catch
			else {// cannot catch
				if (canKick()) {
					bestAction = kickBallAction();
					subStateString = KICK;
					if (debug)
						System.out.println("kick ball " + bestAction);
				}// end can kick
				else {// cannot kick
					if (fastestToBall() && !ballGoesOut) {
						bestAction = goForInterceptAction();
						subStateString = INTERCEPTION;
						if (debug)
							System.out
									.println("fastest && ball not out --> intercept "
											+ bestAction);
					}// end fastest to intercept
					else { // not fastest to intercept
						if (ballInDangerousDistance() && !ballGoesOut) {
							if (teamIsFastToBall()) {
								bestAction = defensivePositioningAction();
								subStateString = DEFENSIVE;
								if (debug)
									System.out
											.println("We are fastest --> defensive "
													+ bestAction);
							}// end team has ball
							else {// team is not fastest to ball
								if (attackerIsBlocked()) {
									bestAction = defensivePositioningAction();
									subStateString = DEFENSIVE;
									if (debug)
										System.out
												.println("attacker blocked --> defensive "
														+ bestAction);
								}// end attacker is blocked
								else {// attacker isn't blocked
									if (Math.abs(ballRef.getPosition().x) < 7) {
										if (attackerCanKick()) {
											bestAction = defensivePositioningAction();
											subStateString = DEFENSIVE;
											if (debug)
												System.out
														.println("attacker can kick --> kick danger action "
																+ bestAction);
										} // end attacker can kick
										else {// attacker cannot kick
											bestAction = defensivePositioningAction();
											subStateString = DEFENSIVE;
											if (debug)
												System.out
														.println("attacker cannot kick --> offensive positioning "
																+ bestAction);
										}// end attacker cannot kick
									} // end ball in front of goal
									else { // ball at side
										bestAction = defensivePositioningAction();
										subStateString = DEFENSIVE;
										if (debug)
											System.out
													.println("ball at side --> defensive positioning "
															+ bestAction);
									}
								}// end attacker isn't blocked
							}// end team is not fastest to ball
						}// end ball in dangerous distance
						else if (ballInDangerousDistance() && ballGoesOut) {
							// possibly a goal
							if (ballGoesToGoal()) {
								bestAction = interceptGoalBallAction();
								subStateString = INTERCEPT_GOAL_BALL;
								if (debug)
									System.out
											.println("possible goal shot --> intercept "
													+ bestAction);
							} else {
								bestAction = defensivePositioningAction();
								subStateString = DEFENSIVE;
								if (debug)
									System.out
											.println("ball goes out --> defensive positioning "
													+ bestAction);
							}
						} else {// ball not in dangerous distance
							bestAction = defensivePositioningAction();
							subStateString = DEFENSIVE;
							if (debug)
								System.out
										.println("ball away --> defensive positioning "
												+ bestAction);
						}// end ball not in dangerous distance
					} // end not fastest to intercept
				}// end cannot kick
			}// end cannot catch
		}// end play_on
		else {// playmode is not play_on
			if (PLAY_MODE.META_STANDARD_OWN.contains(playmode)) { // our
				// standard
				if (goalieStandard()) {
					bestAction = myStandardAction();
					subStateString = STANDARD;
					if (debug)
						System.out.println("goalie standard " + bestAction);
				}// end standard of goalie
				else {// no standard of goalie
					bestAction = defensivePositioningAction();
					subStateString = DEFENSIVE;
					if (debug)
						System.out.println("other standard " + bestAction);
				}// end ball not in penalty
			}// end our standard
			else {// not our standard
				if (playmode == PLAY_MODE.GOAL_OTHER) {
					bestAction = BasicActionFactory.turnBodyToPoint(keeperRef,
							sConf.GOAL_POS_OTHER);
					benefit = 1;
				} else {
					bestAction = defensivePositioningAction();
					if (debug)
						System.out.println("opp standard " + bestAction);
				}
			}// end not our standard
		}// end not play_on
		time = System.currentTimeMillis() - time;
		if (debug)
			System.out.println("time: " + time);
	}

	/**
	 * @return true if ball close enough and in our penalty
	 */
	private boolean canCatch() {

		if (ballRef.getAge(getWorld().getBodyCycle()) > 2) {
			return false;
		}
		if (!getWorld().inOwnPenaltyArea(ballRef)) {
			return false;
		}
		if (keeperRef.getCatchDistance() - CATCH_SECURITY_DIST < ballRef
				.getDistance(keeperRef)) {
			return false;
		}
		return true;
	}

	/**
	 * @return true if the goalie can kick the ball
	 */
	private boolean canKick() {

		if (ballRef.getAge(getWorld().getBodyCycle()) > 2) {
			return false;
		}
		if (keeperRef.getKickDistance() - KICK_SECURITY_DIST < keeperRef
				.getDistance(ballRef)) {
			return false;
		}
		return true;
	}

	/**
	 * @return true if I am fastest to ball
	 */
	private boolean fastestToBall() {

		return prophet.iAmFastestToBall() && getWorld().getAge(ballRef) <= 2;
	}

	/**
	 * @return true if the ball is closer to goal than the given dangerous
	 *         distance
	 */
	private boolean ballInDangerousDistance() {

		if (ballRef.getAge(getWorld().getBodyCycle()) > 3) {
			return false;
		}
		Vektor pos = ballRef.getPosition();
		if (abs(pos.y + this.sConf.half_length) < DANGEROUS_DIST) {
			if (abs(pos.x) < this.sConf.half_goal_width) {
				return true;
			}
			if (pos.getDistance(sConf.GOAL_POST_LEFT_OWN) < DANGEROUS_DIST) {
				return true;
			}
			if (pos.getDistance(sConf.GOAL_POST_RIGHT_OWN) < DANGEROUS_DIST) {
				return true;
			}
		}
		return false;
	}

	/**
	 * @return true if a teammate is fastest to ball
	 */
	private boolean teamIsFastToBall() {

		if (ballRef.getAge(getWorld().getBodyCycle()) > 3) {
			return false;
		}
		if (fastOppIT > prophet.getInterceptionTime() + INTERCEPTION_SECURITY
				|| fastOppIT == -1) {
			return true;
		}
		return false;
	}

	/**
	 * @return true if the attacker has to pass a defender
	 */
	private boolean attackerIsBlocked() {

		if (fastOpp != null) {
			dummyVektor1.x = 10;
			dummyVektor1.y = -52.5;
			dummyVektor2.x = -10;
			dummyVektor2.y = -52.5;
			double angle1 = fastOpp.getAngleTo(dummyVektor1);
			double angle2 = fastOpp.getAngleTo(dummyVektor2);
			if (angle1 < 0) {
				angle1 += 360;
			}
			if (angle2 < 0) {
				angle2 += 360;
			}
			double middleAngle = Vektor.normalize((angle1 + angle2) / 2);
			double dev = Math.abs(angle1 - middleAngle);
			LinkedList<Player> team = getWorld().getTeammates(
					fastOpp.getPosition(), middleAngle, dev, IS_DEFENDED_DIST);
			int index = -1;
			for (Player p : team) {
				if (p.isGoalie()) {
					index = team.indexOf(p);
				}
			}
			if (index > -1) {
				team.remove(index);
			}
			if (team.size() < 1) {
				return false;
			}
			return true;
		}
    return true;
	}

	/**
	 * @return true if the attacker can kick
	 */
	private boolean attackerCanKick() {

		return getWorld().ballCanBeKickedByOpponent();
	}

	/**
	 * @return true if the goalie has to make the standard
	 */
	private boolean goalieStandard() {

		if (playmode == PLAY_MODE.FREE_KICK_OWN
				|| PLAY_MODE.GOAL_KICK_OWN == playmode) {
			return true;
		}
		return false;
	}

	/**
	 * if passing has a better value, the result is unused otherwise we hold the
	 * ball, or kick it out (in case of near opponents)
	 * 
	 * @return a kick-action
	 */
	private Action kickBallAction() {

		if (prophet.getPassBenefit() <= PASS_BENEFIT) {// calculate better kick
			if (fastOppIT < 5 && fastOppIT != -1) {
				Vektor kickPos = ballRef.getPosition().x > 0 ? sConf.CORNER_RIGHT_OWN
						: sConf.CORNER_LEFT_OWN;
				double kickAngle = ballRef.getAngleTo(kickPos);
				benefit = 1.0;
				return new KickAction(100, kickAngle);
			}
      dummyVektor.setVektor(0.8, keeperRef.getBodyDir());
      Vektor kickPos = getWorld().getMyNextPosition()
      		.add(dummyVektor);
      KickAction kick = BasicActionFactory.kickInTime(keeperRef,
      		ballRef, kickPos, 1, 0.2);
      if (kick == null) {
      	double kickAngle = Vektor.normalize(ballRef.getSpeed()
      			.getAngle() + 180);
      	kick = new KickAction(100, kickAngle);
      }
      benefit = PASS_BENEFIT;
      return kick;
		}// end calculate better kick
    benefit = PASS_BENEFIT;
    return prophet.getPassAction();
	}

	/**
	 * makes the goalie catch the ball
	 * 
	 * @return the needed catch-action
	 */
	private Action catchBallAction() {

		double angleToBall = this.getWorld().getSelfRef().getAngleRel(ballRef, RELATIVE.TO_BODY);
		benefit = 1.0;
		return new CatchAction(angleToBall);
	}

	/**
	 * @return the best action to intercept the ball
	 */
	private Action goForInterceptAction() {

		// System.out.println("\n"+getWorld().getCycleAndPlayerString()+"
		// intercept
		// Ball");
		return prophet.getMyNextInterceptAction();
	}

	/**
	 * @return an action that moves the goalie to an defensive position near the
	 *         goal
	 */
	private Action defensivePositioningAction() {

		// System.out.println("\n"+getWorld().getCycleAndPlayerString()+"
		// defpos");
		benefit = 1.0;
		// calculate good pos in goal
		calculateDefensivePosition();

		if (debugDef)
			System.out.println("bestDefPos: " + bestDefPos);

		ra.setArea(1, 0, keeperRef, GOOD_POS_BUFFER);
		Vektor lookPos = Vektor.getDummyInstance();
		lookPos.copy(bestDefPos);

		if (ballRef.getPosition().x < bestDefPos.x) {
			lookPos.x = bestDefPos.x - 3;
		} else {
			lookPos.x = bestDefPos.x + 3;
		}

		if (debugDef)
			System.out.println("turnTo: " + lookPos);

		Action a = ra.getFirstMoveToPos(bestDefPos, GOOD_POS_BUFFER, 15, true,
				-1, ballRef.getPosition(), lookPos);
		if (ballRef.getDistance(sConf.GOAL_POS_OWN) > 30) {
			keeperRef.correctDashPower(a, STAMINA_LEVEL.MAX);
			if (debugDef)
				System.out.println("stamina max");
		} else {
			keeperRef.correctDashPower(a, STAMINA_LEVEL.LOW);
			if (debugDef)
				System.out.println("stamina low");
		}

		return a;
	}

	/**
	 * @return uses goalie kick in state
	 */
	private Action myStandardAction() {

		benefit = 0.001;
		return null;
	}

	private void calculateDefensivePosition() {

		dummyLine1.copyPoint1(ANCHOR);
		dummyLine1.copyPoint2(ballRef.getPosition());
		if (ballRef.getPosition().y < -50) {
			if (ballRef.getPosition().x < sConf.GOAL_POST_LEFT_OWN.x - 3) {
				if (debugDef)
					System.out.println("ball close and at side");
				bestDefPos.x = sConf.GOAL_POST_LEFT_OWN.x + 0.75;
				bestDefPos.y = sConf.GOAL_POST_LEFT_OWN.y + 1.5;
			} else if (ballRef.getPosition().x > sConf.GOAL_POST_RIGHT_OWN.x + 3) {
				if (debugDef)
					System.out.println("ball close and at side");
				bestDefPos.x = sConf.GOAL_POST_RIGHT_OWN.x - 0.75;
				bestDefPos.y = sConf.GOAL_POST_RIGHT_OWN.y + 1.5;
			} else {
				if (debugDef)
					System.out
							.println("ball close and in front --> go to ball");
				bestDefPos.copy(ballRef.getPosition());
			}
		} else {
			// more than 2.5 m from goal
			try {
				bestDefPos.copy(dummyLine1.getPointWithYValue(-50));
				if (debugDef)
					System.out.println("pos on 50-Line: " + bestDefPos);
			} catch (Exception e) {
				if (debugDef)
					System.out.println("exception pos (-50;0)");
				bestDefPos.x = 0;
				bestDefPos.y = -50;
			}
			if (fastOpp != null) {
				if (fastOpp.getPosition().y < -37.5
						&& Math.abs(fastOpp.getPosition().x) < 9) {
					if (debugDef)
						System.out.println("fastOpp in front -> move BestPos");
					dummyVektor2.copy(fastOpp.getPosition());
					dummyVektor1.setVektor(2, fastOpp.getBodyDir());
					dummyVektor2.addToThis(dummyVektor1);
					Line l = new Line(fastOpp.getPosition(), dummyVektor2);
					dummyVektor2.copy(l.getPointWithYValue(-50));
					if (Math.abs(dummyVektor2.x) > 6) {
						if (dummyVektor2.x > 0) {
							dummyVektor2.x = 6;
						} else {
							dummyVektor2.x = -6;
						}
					}
					if (debugDef)
						System.out.println("movePos: " + dummyVektor2);
					bestDefPos.x = (5 * bestDefPos.x + dummyVektor2.x) / 6;
				}
			}
			if (bestDefPos.x > 6.5) {
				bestDefPos.x = 6.5;
				bestDefPos.y = -51;
			} else if (bestDefPos.x < -6.5) {
				bestDefPos.x = -6.5;
				bestDefPos.y = -51;
			}
		}
		if (ballRef.getPosition().x < 0) {
			double ballCycles = (ballRef.getDistance(sConf.GOAL_POST_LEFT_OWN) - 2.7) / 3.;
			if (Math.abs(bestDefPos.x - sConf.GOAL_POST_LEFT_OWN.x) - 0.5 < ballCycles) {
				if (debugDef)
					System.out.println(getWorld().getCycleAndPlayerString()
							+ " ballCycles :" + ballCycles);
				bestDefPos.x = sConf.GOAL_POST_LEFT_OWN.x - 0.5
						+ (ballCycles * 0.8);
				if (debugDef)
					System.out
							.println("too close to left post " + bestDefPos.x);
				if (bestDefPos.x > 0) {
					bestDefPos.x = 0;
				}
			}
		}
		if (ballRef.getPosition().x > 0) {
			double ballCycles = (ballRef.getDistance(sConf.GOAL_POST_RIGHT_OWN) - 2.7) / 3.;
			if (Math.abs(bestDefPos.x - sConf.GOAL_POST_RIGHT_OWN.x) - 0.5 < ballCycles) {
				if (debugDef)
					System.out.println(getWorld().getCycleAndPlayerString()
							+ " ballCycles :" + ballCycles);
				bestDefPos.x = sConf.GOAL_POST_RIGHT_OWN.x + 0.5
						- (ballCycles * 0.8);
				if (debugDef)
					System.out.println("too close to right post "
							+ bestDefPos.x);
				if (bestDefPos.x < 0) {
					bestDefPos.x = 0;
				}
			}
		}
	}

	/**
	 * Calculates the needed turn-argument to reach a given turn-angle.
	 * 
	 * @param a
	 * @param p
	 * @return the needed argument
	 */
	public static double getTurnArg(double a, Player p) {

		return a * (1 + p.getPConf().INERTIA_MOMENT * p.getSpeed().getLength());
	}

	/**
	 * Calculates the resulting turn-angle after using argument a.
	 * 
	 * @param a
	 * @param p
	 * @return the resulting turn angle
	 */
	public static double getTurnFromArg(double a, Player p) {

		return a / (1 + p.getPConf().INERTIA_MOMENT * p.getSpeed().getLength());
	}

	private boolean ballGoesToGoal() {

		Vektor ballpos = getWorld().getBallRef().getPosition();
		Vektor ballspeed = getWorld().getBallRef().getSpeed();
		if (Math.abs(ballspeed.getAngle()) <= 91) {
			return false;
		}
		dummyLine1.copyPoint1(ballpos);
		dummyLine1.copyPoint2(ballpos.add(ballspeed));
		Vektor outPoint = dummyLine1.getPointWithYValue(-52.5);
		if (Math.abs(outPoint.x) < 8) {
			return true;
		}
		return false;
	}

	private Action interceptGoalBallAction() {

		ra.setArea(1, 0, getWorld().getSelfRef(), getWorld().getSelfRef()
				.getCatchDistance());
		dummyLine1.copyPoint1(ballRef.getPosition());
		dummyLine1.copyPoint2(ballRef.getPosition().add(ballRef.getSpeed()));
		dummyLine2.copyPoint1(keeperRef.getPosition());
		dummyVektor.setVektor(10, getWorld().getBodyAngle());
		dummyLine2.copyPoint2(keeperRef.getPosition().add(dummyVektor));
		boolean dash = true;
		Vektor outPoint = dummyLine1.getPointWithYValue(-52.5);
		Vektor dashPoint = dummyLine1.getIntersectionPointWithLine(dummyLine2);
		if (dashPoint.getDistance(outPoint) > ballRef.getDistance(outPoint)) {
			dash = false;
		}
		double dashTime = ballRef.getDistance(dashPoint)
				/ ballRef.getSpeed().getLength();
		double outTime = ballRef.getDistance(outPoint)
				/ ballRef.getSpeed().getLength();
		if (dash) {
			dashTime -= ra.getMovesToPos(dashPoint, true, null).size();
			outTime -= ra.getMovesToPos(outPoint, true, null).size();
			if (outTime > dashTime) {
				dash = false;
			}
		}
		if (dash) {
			return ra.getFirstMoveToPos(dashPoint,
					keeperRef.getCatchDistance(), 15, true, -1, null, null);
		} else {
			return ra.getFirstMoveToPos(outPoint, keeperRef.getCatchDistance(),
					15, true, -1, null, null);
		}
	}
}
